/***********************************************************************
 * $Id:: BLDC.c 3871 2010-07-16 11:50:22Z gerritdewaard                $
 *
 * Project: LPC1100 BLDC Motor Control AN  
 *
 * Description:
 *			
 *     
 *
 ***********************************************************************
 * Software that is described herein is for illustrative purposes only
 * which provides customers with programming information regarding the
 * products. This software is supplied "AS IS" without any warranties.
 * NXP Semiconductors assumes no responsibility or liability for the
 * use of the software, conveys no license or title under any patent,
 * copyright, or mask work right to the product. NXP Semiconductors
 * reserves the right to make changes in the software without
 * notification. NXP Semiconductors also make no representation or
 * warranty that such application will be suitable for the specified
 * use without further testing or modification.
 **********************************************************************/

#include "application.h"


volatile MOTOR_TypeDef Motor;
//= {
//	/****************************************************/
//	/* Default PID structure initialization				*/
//	/****************************************************/
//	/* PID_Motor->p = */			20,
//	/* PID_Motor->i = */ 			7,
//	/* PID_Motor->d = */ 			0,
//	/* PID_Motor->sp = */			500, // RPM
//	/* PID_Motor->pv = */ 			0,
//	/* PID_Motor->err[0] = */		0,
//	/* PID_Motor->err[1] = */ 		0,
//	/* PID_Motor->err[2] = */ 		0,
//	/* PID_Motor->mv = */ 			0,
//	/* PID_Motor->HALstate[0] = */ 	0,
//	/* PID_Motor->HALstate[1] = */ 	0,
//	/* PID_Motor->TMRval[0] = */ 	0,
//	/* PID_Motor->TMRval[1] = */ 	0,
//	/* PID_Motor->Deadtime = */ 	0,
//	/* PID_Motor->Direction = */ 	CCW,
//	/* PID_Motor->CMT_step = */ 	1,
//	/* PID_Motor->Startup = */ 		35,
//	/* PID_Motor->max_mv = */ 		0, // (motor_period*0.90),
//	/* PID_Motor->PolePairs = */ 	1,
//	/* PID_Motor->Brake = */		0,
//	/* PID_Motor->Enable = */		0,
//	/* PID_Motor->RPM = */ 			0
//							};

uint8_t CMT_tbl[2][8] = {{0xF, 2, 0, 1, 4, 3, 5, 0xF},
						 {0xF, 5, 3, 4, 1, 0, 2, 0xF}};

/* BLDC specific variable definitions */
volatile uint32_t bldc_Tick = 0;
volatile uint32_t bldc_SySTickold = 0;
volatile uint32_t bldc_SySTicknew = 0;

volatile uint32_t loadval_tmp = 0;
//volatile uint32_t

/******************************************************************************
** Function name:		
**
** Descriptions:		
**
** parameters:			
** Returned value:		
** 
******************************************************************************/
void vBLDC_Init (MOTOR_TypeDef *ptr)
{

	ptr->max_mv = MOTOR_MAX_PERIOD;

	/****************************************************/
	/* Default PID structure initialization				*/
	/****************************************************/
	ptr->Kp = 5;
	ptr->Ki = 2;
	ptr->Kd = 0;
	ptr->sp = 3000;	   	 // RPM
	ptr->pv = 0;
	ptr->IntError = 0;
	ptr->LastError = 0;
	ptr->mv = 0;
	ptr->HALstate[0] = 0;
	ptr->HALstate[1] = 0;
	ptr->TMRval[0] = 0;
	ptr->TMRval[1] = 0;
	ptr->Deadtime = 0;
	ptr->Direction = CW;
	ptr->CMT_step = 1;
	ptr->Startup = 35;
	ptr->max_mv = MOTOR_MAX_PERIOD;
	ptr->PolePairs = 1;//4;
	ptr->Brake = 0;
	ptr->Enable = 0;
	ptr->RPM = 0;
	ptr->RampingUp = TRUE;

	/****************************************************/
	/* PWM (FET drivers)								*/
	/****************************************************/
	/* PIO0_1	PHASEA_H  CT32B0_MAT2					*/
	/* PIO0_11	PHASEA_L  CT32B0_MAT3					*/
	/* PIO0_8	PHASEB_H  CT16B0_MAT0					*/
	/* PIO0_9	PHASEB_L  CT16B0_MAT1					*/
	/* PIO1_1	PHASEC_H  CT32B1_MAT0					*/
	/* PIO1_2	PHASEC_L  CT32B1_MAT1					*/
	/****************************************************/
	/* Setup Phase A high and low side PWM signals */
	//init_timer32PWM(TIMER0, MOTOR_PERIOD, (MATCH0 | MATCH2 | MATCH3));
	/* Setup Phase B high and low side PWM signals */
	//init_timer16PWM(TIMER0, MOTOR_PERIOD, (MATCH0 | MATCH1) , CAP_DISABLE);
	/* Setup Phase C high and low side PWM signals */
	//init_timer32PWM(TIMER1, MOTOR_PERIOD, (MATCH0 | MATCH1));

	/* Make sure no match occurs */
	vBLDC_Stop ();
	
	/* enable all timers for motor driving */
	//enable_timer16(TIMER0);
	//enable_timer32(TIMER0);
	//enable_timer32(TIMER1);
	
}


/******************************************************************************
** Function name:		bldc_stopPWM
**
** Descriptions:
**
** parameters:
** Returned value:
**
******************************************************************************/
void vBLDC_Stop (void)
{
}

/******************************************************************************
** Function name:		bldc_CalcRPM
**
** Descriptions:
**
** parameters:
** Returned value:
**
******************************************************************************/
void vBLDC_CalcRPM (MOTOR_TypeDef *ptr )
{
	uint32_t currVal = LPC_TMR16B1->TC, deltaTicks = 0;

	/* Get the number of timer ticks passed since last RPM
	 * calculation */
	if (currVal > ptr->TMRval[0])
		deltaTicks = currVal - ptr->TMRval[0];
	else
		deltaTicks = 0xFFFF - (ptr->TMRval[0] - currVal);

    /* Convert the timer value to a RPM value */
    ptr->RPM = (((uint32_t)SystemCoreClock/LPC_TMR16B1->PR)* 60 )/(2 * deltaTicks * ptr->PolePairs);    		// convert to RPM

    /* Store the currVal for next RPM Calc */
	ptr->TMRval[0] = currVal;
}

void vBLDC_RampUp (MOTOR_TypeDef *ptr, uint32_t max_RPM)
{
	static uint8_t force;
	static uint32_t duty;

	/* Check whether motor should ramp up */
	if (ptr->RampingUp && ptr->Enable)
	{
		duty += 50;
		ptr->mv = duty;
	}
	if (ptr->RPM >= (max_RPM))
	{
		ptr->RampingUp = FALSE;
		duty = 0;
	}
	return;
}


